#include "structure.h"

inline double normalize_theta_rad(double theta);
inline double normalize_theta_deg(double theta);
LocalizerParticleFilter *localizer_particle_filter_new(int num_particles);

int compare_particle(const void * a, const void * b);

void localizer_particle_filter_delete(LocalizerParticleFilter *filter);
void localizer_initialize_particles_gaussians(LocalizerParticleFilter *filter,
					      int num_modes,
					      Point *mean,
					      Point *std);
void localizer_initialize_particles_gaussian(LocalizerParticleFilter *filter,
					     Point mean, 
					     Point std);
double likelihood_field_range_finder_model(LaserRangeData *laser, 
					   LocalizerParticle pose, 
					   PointMap *map);
double likelihood_field_range_finder_model(LaserRangeData *laser, 
					   LocalizerParticle pose, 
					   OccupancyGridMap *map);

// 等分散モデルによるサンプリング
void localizer_particle_resample(LocalizerParticleFilter *filter);

// オドメトリ動作モデルによるサンプリング
void sample_motion_model_odometry(LocalizerParticleFilter *filter,
				  LocalizerOdometry *odometry,
				  LocalizerOdometry *prev_odometry);
